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ABSTRACT 


It is generally agreed that some type of vision system capable of providing a 
preview of terrain is an important attribute of a driverless vehicle. One approach 
to providing this capability is to use active images such as those obtained by 
sonar or radar. This thesis is concerned with a computer simulation study of an 
approach to data processing for range images obtained from an optical radar 
system using a scanning laser beam. The system studied is modeled after the 
ERIM scanner mounted on the Adaptive Suspension Vehicle walking machine 
developed at Ohio State University. Both the problem of registering successive 
images in the presence of vehicle motion and of optimally averaging such images 


to obtain more accurate terrain elevation data are investigated in the thesis. 
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I. INTRODUCTION 


Robots have long been in man’s thinking. From the early science fiction 
writers to today’s cartoons, the idea of robots that operate on their own has 
fascinated us. There have been, and still are, many research programs concerned 
with realization of an autonomous robot. These programs include wheeled 
machines, tracked vehicles, and legged vehicles. 

There are many uses and advantages to each type of robotic machine. Legged 
vehicles or walking machines have an advantage over wheeled or tracked vehicles 
in that they can traverse a greater variety of terrains. A walking machine can 
traverse terrain that is heavily wooded, can walk over muddy terrain, and can 
negotiate obstacles that a wheeled or tracked vehicle may not be able to 
overcome. However, before any robotic machine can travel anywhere by itself, it 
has to be able to see where it is going and decide where it wants to go based on 
what it sees. Once all of these research areas have been mastered, widespread 


application of autonomous robots may follow. 


A. GOALS 

The goal of this thesis is to explore methods to obtain accurate terrain 
altitude information that may be used by some type of route planning program 
for an autonomous vehicle. The process of obtaining these altitude values will 
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include some type of filtering of noisy information and correction of errors in the 
vehicle’s location information. The sensors involved in this process are assumed 
to be some type of inertial navigation system (INS) and an optical radar scanner. 

The vehicle used as a reference for setting up the model used in this thesis is 
the Ohio State University Adaptive Suspension Vehicle (ASV), which is a six- 
legged walking machine. The ASV is equipped with an optical terrain scanner 
manufactured by the Environmental Research Institute of Michigan (ERIM). 
[Ref. 1] 

A secondary purpose of this thesis is to develop a simulation model for the 
optical scanner and terrain that will be used in the testing of the techniques 
presented in the thesis. These models could also be used in future studies that 


involve a scanner and terrain. 


B. ORGANIZATION 

Chapter II presents a discussion of work dealing with early robotic systems as 
well as current projects under development. The optical terrain scanner used as a 
reference in this thesis is discussed in some detail. Finally, a general discussion on 
regression analysis and Kalman filtering are presented. 

The reason for obtaining an accurate terrain altitude map is discussed in 
Chapter III. A detailed discussion is presented on how the inertial navigation and 
optical scanner data are mathematically manipulated to obtain the x and y 


coordinates and the terrain altitude of the terrain being scanned. Finally, a more 


detailed discussion of Kalman filtering and regression analysis as they apply to 
this thesis are presented. 

Chapter IV presents the simulation models used for the terrain and the 
optical scanner. Also, flow charts showing how data is handled during the 
simulation runs are included. 

Finally, Chapter V discusses the simulation experiments conducted and their 
results. The last chapter, Chapter VI, puts forward some conclusions about the 


work presented in this thesis followed by some recommendations for future work. 


Il. REVIEW OF PREVIOUS WORK 


A. INTRODUCTION 

For many years, man has been working toward the goal of making an 
autonomous robot. However, before a robot or machine can be truly autonomous, 
it has to have some way of seeing the world in which it is to operate. In this 
chapter, past robotic systems and their vision systems are reviewed along with 


ways of handling the data from these vision systems. 


B. MOBILE ROBOT SYSTEMS 
1. Early Robot Systems 

Inventors have been fascinated for centuries with the idea of creating 
machines that act like animals or human beings. The earliest of these were 
"clockwork" machines like the "artificial duck" built by Jacques de Vaucanson in 
the 1730’s, and Baron Wolfgang von Kempenlen’s chess-playing automaton of the 
late eighteenth century (later proved a hoax). Clockwork machines, as the name 
implies, were based on complex mechanical gears and linkages which provided the 
timing and required movements for the machine. Until the advent of digital 
computers and component miniaturization technology, robots remained little 


more than mechanical novelties. [Ref. 2: pp. 254-264] 
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In the 1960’s, researchers at Johns Hopkins University built what could 
possibly be considered the first truly autonomous mobile robot. The sole activity 
of this robot was to navigate the hallways of the laboratory "searching" for 
electrical power outlets to keep its batteries charged. Special purpose circuitry 
(rather than a programmable computer) was used to direct the robot’s actions, 
and information from an elementary sonar system kept the robot centered 
between the walls. Although simplistic, the machine demonstrated an ability to 
interact with its enviroment and to sustain itself through its own actions (i.e. 
searching for outlets and recharging its batteries). [Ref. 2: pp. 254-264] 

The next major step in autonomous robot development was accomplished 
in 1969 at the Stanford Research Institute (SRI). A mobile robot dubbed 
"Shakey" was linked to a digital computer via radio giving it the much needed 
additional "brainpower" to more fully interact with its enviroment and solve 
relatively complex problems. Using a television camera as a sensor and 
hierarchical software control, Shakey was able to negotiate obstacles and 
accomplish simple path planning when moving from one location to another. 
This first attempt at machine interaction with an uncertain enviroment 
encountered many problems. Rather complex image processing algorithms were 
required to discern potential obstacles and complicated computer programs were 
needed to map the obstacles onto a spatial grid coordinate system that the robot 
could "understand". Shakey kept track of its own position through a crude "dead 


reckoning" system in which sensors counted the number of rotations of the robot’s 
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wheels to determine how far it had moved from its initial starting position. It was 
soon discovered that, while multiple sensors can provide a more comprehensive 
representation of the enviroment, coordinating the incoming data from those 
sensors represents a major hurdle. For instance, on the Shakey robot, wheel 
slippage caused position errors which in turn induced grid map drift and resulted 
in multiple representations of individual obstacles cluttering the map. [Ref. 3: pp. 
10-15] 

For all its problems, the Shakey robot remained state-of-the-art until the 
mid-1970’s. During this decade, research involving incorporation of new 
technologies into autonomous robots continued. Advances in _ integrated 
electronics manufacturing made possible relatively powerful single-board 
computers and smaller, more compact sensor systems. Still, many of the robots 
built during this time frame were tethered to an off-board computer either via 
cables or radio link. Among the robots of this period were the Jet Propulsion 
Laboratory’s Mars Rover, the Stanford Cart, and France’s Laboratoire 
d’Automatique et d’Analyse des Systemes’ HILARE robot. These robots all 
shared the common goal of integrating multiple sensor systems and developing 
algorithms to allow autonomous operation in uncertain enviroments. [Ref. 3: pp. 
20-25] 

The JPL Rover used a laser range finder, stereo TV cameras, tactile, and 
proximity sensors to observe its enviroment, and a gyrocompass and optical 


encoders on the wheels to keep track of its own position. Like the Shakey robot, 
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sensor integration and error propogation problems plagued the system and, due to 
the heavy computing requirements, the robot was dependent on its mainframe 
computer link. [Ref. 3: pp. 20-30] 

The Stanford Cart employed a complex image processing scheme to 
improve obstacle recognition. A TV link to an off-board computer and a 
moveable, slide-mounted camera system were used to reduce imaging errors, but 
this system proved slow and very sensitive to light and shadow effects. Like 
previous robots, the Stanford Cart’s dead reckoning system induced errors into 
the system which it could not overcome. (Ref. 3: pp. 20-25] 

The French Hilare robot incorporated a number of techniques most often 
associated with artificial intelligence. "Expert system" modules worked together, 
sharing information about navigation, obstacle identification, etc., under a 
computing hierarchy of onboard microcomputers linked to an _ off-board 
minicomputer and mainframe. The Hilare used a laser range finder and a 
prediction algorithm to anticipate what the obstacle map would "look" like after 
the robot moved. It could then correct its perception and/or position information 
to merge the predicted and actual image information and thereby alleviate many 
of the errors experienced in the earlier systems. |Ref. 3: pp.20-25] 

A departure from wheeled vehicles is the Ohio State University (OSU) 
Hexapod. The Hexapod is a six-legged walking machine which first operated in 
1977. Although the OSU Hexapod is strictly a laboratory robot, a truly 


autonomous walking machine, like its animal counterpart in nature, would be 
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ideally suited to traversing rough, unpredictable terrain. The OSU machine’s legs 
are controlled to provide static stability; that is, at least three legs are on the 
ground providing a stable supporting tripod for the robot at all times. The 
robot’s sensors include stereo TV cameras and motion sensors to maintain stable 
body position while moving or climbing. While sophisticated computer routines 
automate many of the Hexapod’s balance, leg movement, and coordination 
functions, the Hexapod remains tethered to its external power supply and 
minicomputer and relies on human interaction for navigation and for some foot- 
placement decisions. [Ref. 4: pp. 3-17] 
2. Autonomous Land Vehicles 

The Autonomous Land Vehicle (ALV) is the newest, and thus far the 
most successful, of the various autonomous wheeled robots which have been tested 
to date. Designed for the Defense Department’s Advanced Research Projects 
Agency (DARPA), the vehicle is a hydraulically powered, eight-wheeled platform 
equipped with the computers and sensors required to negotiate unfamiliar 
territory without human intervention. The ALV’s mission is conceptually more 
difficult than the robots previously described since it is designed to operate 
outdoors in a much more unpredictable enviroment. The ALV’s sensors include a 
TV camera and optical radar for sensing the enviroment, and an inertial 
navigation system for position information. During initial testing, the sensor data 
was successfully integrated and processed, providing the vehicle with "road 


following" information able to keep it between ditches. [Ref 5] 
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Another ALV was built in 1985 by FMC Corporation. This vehicle is an 
armored personnel carrier which is a tracked, instead of wheeled vehicle. This 
ALV has an inertial navigation system, a vehicle control computer, and a sonic 
imaging sensor. The architecture of the FMC vehicle consists of 5 subsystems 
called a Planner, Observer, Mapmaker, Pilot, and Vehicle Control. In general, 
the Planner plans the route of the ALV using preloaded digitized maps of the 
local terrain. The Observer uses the sensors input to create an obstacle map, then 
the Mapmaker generates a pilot map using the information from the Planner and 
the obstacle map. The Pilot uses the pilot map and guides the vehicle along an 
optimum path by passing instructions to the Vehicle Control subsystem. The 
first test of the FMC ALV was conducted in 1985 in which it successfully avoided 
obstacles and performed path execution at a speed of 5 mph. [Ref. 6: pp. 14-23] 

3. DARPA Adaptive Suspension Vehicle 

A more advanced six-legged walking machine, called the Adaptive 
Suspension Vehicle (ASV), is currently undergoing test and evaluation at Ohio 
State University. The ASV differs from the OSU Hexapod in that it is completely 
self-sufficient (no external power or computing required). The ASV uses an 
internal combustion engine to provide power to its hydraulic systems and on- 
board computers. The ASV carries its own computers that control leg movement 
and stability, and provide vision information. The vision system consists of an 
optical radar mounted on top of the control cab. At present, the ASV requires an 


on-board human operator to plan and control the motion of its body using a 
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three-axis control stick. With continuing research, the ASV could become the 


first autonomous legged walking machine. (Ref. 1: pp. 1-60] 


C. DESCRIPTION OF OPTICAL RADAR 
1. Physical Description 
The ASV is equipped with an optical terrain scanner. The optical 
scanner is manufactured by the Environmental Research Institute of Michigan 
(ERIM). The scanner is 26 inches wide, 12.5 inches high and weighs 75 pounds 
[Ref.1: pg. 35]. The optical scanner consists of a scanning mechanism, transmitter 
optical train, and receiver optical train. The scanning mechanism consists of a 
nodding. mirror and a Ronveatited polygon mirror which combine to scan up and 
down and left and right. The transmitter optical train consists of a GaAlAs laser 
diode, collimating lens, anamorphic prism pair and a beam expansion telescope. 
(Ref. 7: pp. 3-4]. 
2. Performance Data 
The optical scanner has a scan rate of 2 Hertz. The horizontal field of 
scan is +40 to -40 degrees in 128 increments while the vertical field of scan is from 
-15 to -75 degrees elevation in 128 increments. The instantaneous field of view is 
0.5 degrees and the range resolution is 0.125 feet. The maximum horizontal range 
is 32 feet. [Ref. 7: pp. 2-3] 
Due to various factors such as weather, obstacles and equipment errors, 


the range value returned by the ERIM scanner is not exact [Ref. 7: pg. 5]. The 
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ASV has or will have gyro or magnetic compasses for azimuth, elevation and roll 
inputs along with an altimeter for altitude information and geoposition satellite 
(GPS) inputs for horizontal position. Even though the ASV will be able to 
receive position inputs from GPS and the altimeter, they are inaccurate. That is, 
although the gyro’s give angles within fractions of a degree, the errors associated 
with GPS and the altimeter are of the order of tens of feet; therefore, x, y, and z 
need to be corrected. Detailed information regarding these two sources of terrain 


Mapping error was not available at the time of writing of this thesis. 


D. ESTIMATION TECHNIQUES 
r. Regression Analysis 
In regression analysis, a number of noisy measurements of a variable are 
used to approximate a functional relationship. Using the notation of Ref. 8, if the 


measured variable of the system is 


Yo(t) = y(t,eo) + €(t) (2.1) 


then the objective of regression analysis is to find a vector, ¢ , which approximates 
the true parameter vector, ¢, in the presence of the measurement error, « . 
Typically, this is accomplished by minimization of some type of sum squared error 
function. 

If the response of the system under consideration can be represented 


linearly as 
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j,= At, +é (2.2) 


where y, is a vector of samples of y,, A is the coefficient matrix, ¢, is the true 
parameter vector, and ¢ is a random error vector, then the sum squared error 


function , ®, can be defined as 
® = (y, — Az) (y, — Ac) ((2e3)} 


where ¢ is the trial parameter vector. To minimize © , the partial derivatives of 6 
with respect to each component of ¢ are equated to zero. Applying this to Eq. 


(2.3), the result is 


ao 
= yb = -2A7G, + 2A" AE = 0 (2.4) 
ae 


OT 
A AC=A jy (2.5) 


Solving Eq. (2.5) for the least squares estimate of the true parameter vector, the 


result 1s 
= [AA] AY, . (2.6) 
This is the least squares parameter estimate of the true parameter, ¢. [Ref. 8: pp. 
65-68] 
2. Iterative Methods 


The Kalman filter is an iterative method used to obtain an optimal 


estimate of a variable in an environment that has noise present. Unlike regression 
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analysis, in using a Kalman filter, it is not necessary to store past measurements 
for present or future computations. For the purposes of this discussion, the 
notation in Ref. 9 is used. It is assumed the reader has some knowledge of 
Kalman filtering. 


Assume a system is described by 


tos ®, 7, -) + WH, (2.7) 
and 
Zz, = Hz, + 3, (2.8) 


where z, is the state variable at time ¢,, , , is the transition matrix at time t,_,, 
w,, and v, are the random noise vectors with zero mean and covariances Q, and 
R,, respectively, z, are the measurements, and H, is the observation matrix. An 
updated estimate of the state, z,(+), based on the measurement 2, and the past 
estimate, z,(—), can be obtained from the recursive form 


Z,(+) os K,2,(-) + K,2, (2.9) 


where a and K, are the time-varying weighting matrices (Kalman gain matrices). 


(Ref. 9: pp. 60-110] 


If z, denotes the estimation error, such that 


Z,(+) =z, + z,(+) (2.10) 


and 
z,(—) See ee z,(—) (2.11) 


then Eqs. (2.8-2.11) yield 
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z,(+) = [A, + K,A, aed I \@e + K,2,(-) + K,v, e 


(2.12) 


By definition, E/%,] = 0 and if E[z,(—)] = 0, this estimator will be unbaised for any 2, 


if 
K, + K,H,-1=0. 
Thus, Eqs. (2.10) and (2.12) become 


t,(+) = z,(—) + K,[%, - H,2,(—)] 


and 
#,(+) = (1 - K,H,)2,(-) + Ka, - 
By definition, the error covariance matrix, P,, is given by 
P,(+) = El2,(+)2,(+) ‘1 
and 


P,(-) = Elz,(-)2,(-)"] - 
If it is assumed that the measurement errors are uncorrelated, then 
E\z,(-)¥,] = Elv,z,(-)"] = 0 . 
Applying Eq. (2.12) to Eqs. (2.16) and (2.17) 
P,(+) = (1 - K,H,)P,(-)(1 — K,H,)" + K,R,K/ . 
The Kalman gain matrix, K,, 1s defined as 
K, = P,(-)H,[H,P,(-)H#, + RJ. 


Substituting Eq. (2.20) into Eq. (2.19) 
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(2.13) 


(2.14) 


(2.15) 


(2.16) 


(2.17) 


(2.18) 


(2.19) 


(2.20) 


Paper (2 =, |, (=) - (2.21) 


From Eqs. (2.14), (2.20) and (2.21), a recursive filter can be set up to obtain the 


optimal estimates of z,(+) [Ref. 9: pp. 60-110]. 


E. SUMMARY 

As has been discussed in this chapter, there have been in the past and are 
now in progress, many projects to develop autonomous vehicles. To become 
autonomous, such a vehicle ought to have a vision system and one such system 
was described. In the next chapter, methods for handling the vision information 


are applied to the problem addressed by this thesis. 
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II. PROBLEM STATEMENT AND PROPOSED SOLUTION METHOD 


A. INTRODUCTION 

Before a vehicle can become autonomous, it needs to know what the terrain it 
is operating in looks like then be able to pick a path it wants to travel along. In 
this chapter, some path selection techniques are discussed in general terms along 
with some detailed discussion on filtering of terrain data. 

For the remainder of this thesis, when reference is made to an optical radar 
scanner,-the ERIM discussed in Section C of Chapter II is assumed to be the one 
in use. This assumption will show up in performance data assumed for the data 


conversions and simulation. 


B. IMMEDIATE-AREA TERRAIN SENSING AND PATH SELECTION 

The need for information about the terrain in the immediate area of a 
walking machine in crucial. As long as the machine has a human driver, he can 
navigate the terrain manually, picking where to walk and, if necessary, placing 
the feet in appropriate positions. However, before a walking machine can become 
truly autonomous, it needs to be able to navigate on its own and pick its own 
path to walk along. Moreover, even in the case of a manned vehicle, it is highly 
desirable that the driver be concerned only with control of the body of the vehicle 
with foothold selection being accomplished automatically from vision data. 


22 


There have been many approaches and schemes for solving the problem of 
selecting adequate footholds and avoiding obstacles. The Autonomous Land 
Vehicle (ALV) uses a vision system that is a combination of a color video TV 
camera and the ERIM optical radar scanner [Ref. 5: pp. 19-23]. The color camera 
detects differences in color between the road and off-road areas, while the optical 
scanner is used strictly to detect differences in smoothness between the two areas. 
A major disadvantage of this system is that the color camera is greatly affected by 
changes in lighting and weather conditions. 

Another obstacle avoidance system is used in the FMC ALV. In that system, 
the Observer uses information from a sonic image sensor to detect obstacles not 
already known on the stored digitized map and then creates an obstacle map. 
The sonic image sensor is a phased array sensor that has a range of 32 meters and 
an arc of 120 degrees. The sensor utilizes sonic waves and produces a map every 
0.2 seconds. {Ref. 6: pp. 14-23] 

With a vision system consisting of just an optical radar terrain scanner, the 
problem of autonomous navigation becomes even more difficult. Before an 
"optimal" path can be picked to walk along, accurate information about the 
terrain altitude in the immediate area has to be known. Once this information 1s 
available. a scheme can be used to decide what areas can be walked over and 
what areas cannot. 

One possible terrain classification scheme was developed by Poulos in Ref. 10. 


In his work, a least squares quadratic surface is fitted to a surface pixel and its 
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eight neighbors. From the x, y, z information of each pixel, he uses the gradient 
vector and Hessian matrix and solves for the eigenvalues of the Hessian matrix. 
From these solutions, he classifies each pixel as a saddle, depression, ridge, plane, 
valley, hill or pass [Ref. 10: pp. 50-69]. To further refine the terrain picture, he 
also takes into account the slope associated with a pixel. Depending on what 
criteria is set for what is a safe slope to traverse, each pixel is also classified as 
being level or having a safe or unsafe slope. The property of being level, safe 
slope, or unsafe slope is called the primary terrain cell classification, while the 
above mentioned categories derived from the Hessian matrix are called the 
secondary classification [Ref. 10:pp.70-78]. 

With each terrain cell classified, it becomes possible to use some type of 
artificial intelligence technique to select the "optimal" path to traverse [Ref.11]. 
The only problem is to make sure that the terrain information provided to the 


classifying scheme is the best and most accurate possible. 


C. TERRAIN SENSING USING THE OPTICAL RADAR 

To be able to use the information obtained from the optical scanner, the data 
must first be converted into Cartesian coordinates. This conversion of the data 
will allow manipulation of any prior or future information that is not in the same 
data format as the incoming data [Ref. 7: pp. 2-3]. In order to convert the optical 


scanner information into Cartesian coordinates, it 1s necessary to use not only the 
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scanner information, but also the information from the Inertial Navigation System 
(INS). 

To do the scanner data conversion, it was decided to model the INS and 
optical scanner systems as a nine link manipulator and to use the Denavit- 
Hartenberg (D-H) transformation for this purpose [Ref 12:.pp. 36-41]. The inputs 
from the INS system consist of an x, y, z translation from the INS origin, and the 
body azimuth, elevation and roll angles. The inputs provided by the optical 
scanner are scanner elevation and azimuth angles and range to the terrain. The 
first three links and the last link of the model are translational transformations, 
while the others are rotational transformations. Figure 1 is the representation of 
the resulting nine link manipulator. Table 1 explains what each value in Figure 1 
represents. 

In drawing Figure 1 and setting up the D-H transformation matrices, the 
following assumptions were made: [Ref. 12:pp.36-41] 

1. The reference coordinate system for the ASV body is positive z down, positive 

x out of the front of the vehicle, and positive y out the right side. 

2. The z,_, axis lies along the axis of motion of the ith joint and the z, axis is 
normal to the z,_, axis. 

3. When possible, the direction of the ith twist axis, z,, associated with a 
particular link in the manipulator model is picked to make the twist angle 


(a,) positive. 
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Figure 1 
Nine Link Manipulator Representation 
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TABLE 1 
D-H TRANSFORMATION SYMBOL MEANINGS 


-—s 


Symbo Represents 
displacement from the INS origin in the z direction 
displacement from the INS origin in the x direction 
displacement from the INS origin in the y direction 
ASV body azimuth angle 
ASV body elevation angle 
ASV body roll angle 


scanner elevation angle 





scanner azimuth angle 
range to terrain 


distance between INS origin and scanner origin 





=) 


of : : : aaanaal. 


distance between scanner mirrors 


a SR 


The: following defmitions are needed for the discussion of the D-H 
transformation matrices for link i: [Ref. 12:pp.36-41| 

1. Link length (a,) is the linear displacement from the inboard motion axis to 
the outboard motion axis along the twist axis, (z,). 

2. Twist angle (a,) is the angular displacement of the outboard motion axis, (z,), 
from the inboard motion axis, (2, ,), about the twist axis, (z,). 

3. Joint displacement (d,) is the linear displacement of the outboard twist axis, 
(z,), from the inboard twist axis, (z,_,), measured along the inboard motion 
aseis, (2._,). 

4. Rotation angle (9,) is angular displacement of outboard twist axis,(z,), from 


the inboard twist axis, (z,,), measured about the inboard motion axis, 


fea 


Using these assumptions and definitions, Table 2 shows the joint and link 
parameter values for the ASV scanner system. The values in Table 2 in 
parentheses are for the reference configuration shown in Figure 1 while the others 
are fixed values. 

Using the information in Table 2, the general D-H transformation matrix, Eq. 


(3.1), can be used to form each link as follows: 


cO, —ca,s9, sa;s9. acO. 


sO. ca,cO, —sa,cO, asO, 


In Eq. (3.1), “4, is the D-H transformation matrix for link i going from origin (i- 


1) to origin i. Also, cO; represents cos©, and s®©, represents sinO@,. To obtain the 


TABLE 2 
LINK AND JOINT PARAMETER VALUES 


Link i 
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Cartesian coordinates of the terrain at the end of the scanner beam, each 
transformation matrix is multiplied in order as shown in Eq. (3.2). 


"Ag= A, 2 'A,z°A,z A,z‘A,z Agzt’A,Z "A, = Ag (Sez) 


In order to simulate the scanning of terrain on the graphics computer, the 
matrix °A, which specifies the position of the ASV body is computed and then 
multiplied by the matrix EAC to get the coordinate transformation for the beam 
tip. Using Eq. (3.1) and Table 2, the following results are obtained 


c4c5c6+5486 ¢c485 c4c5s6—84c6 d, 


64c5c6—c4s6 8465 54¢586+c4c6 d, 


"Ag = : (3.3) 
: 65c6 —¢5 8586 ae 


0 0 0 1 


and 


cic8 —s87 c788 d,c7s8+aicT 


sic8 cT 8788 dys7s8+a7s7 
°Ay = (3.4) 


In Eqs. (3.1), (3.3), and (3.4), the notation has again been abbreviated so that, for 
example, c7 stands for cos®, and s7 stands for sinO,. Also, the origins of the 
scanner and the INS system are assumed to be in the same place, thus making a, 
= 0. While this is in fact not the case for the ASV, this assumption simplifies the 
computations needed to evaluate Eq. (3.2) and at the same time has no effect on 


the validity of the simulation studies carried out in the work of this thesis. 
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D. PROPOSED METHOD OF DATA FILTERING 

Under the present scheme used by Ohio State University, the ASV scans the 
terrain using the optical radar scanner, and using whatever information the INS 
provides, a terrain map is calculated and stored. On all successive scans, the 
terrain map is recalculated with the given data, and the resulting new terrain 
elevation values replace the old ones. Each map is stored using the x, y, and z 
values calculated using the noisy range values provided by the optical radar 
scanner, which makes these x, y, and z values incorrect. The following discussion 
presents an approach to correcting for the z value errors separately from the x and 
y value errors. It is proposed to use Kalman filtering for the z values and 
regression analysis for the x and y values. 

1. Stationary Walker Case 

In the case where the ASV is standing still, the only significant errors 

introduced into the system are the noisy range values from the optical radar 
which in turn affect the calculated values of the altitude, z. In this case, to obtain 
the optimal values of the altitude for each terrain cell, Eqs. (2.14), (2.20) and 
(2.21) can be used. If the assumption is made that H, in Eq. (2.14) is equal to 1, 


then Eq. (2.14) becomes 
2,(+) - 2,(—) i K,|, ‘a 2,(—)] (3.5) 


where 2,(+) is the estimate of the terrain altitude after the range measurement. 


Also, 2,(—) is the estimate before the measurement and K, is the Kalman gain 
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matrix. Finally, 7,’ is the value of the altitude calculated using the noisy range 
measurement. 

From Eq. (2.20), P,(—) is defined as the variance of the error before the 
measurement and R, is the variance of the measurement. For the purposes of this 
problem, R, is the same as the variance of the altitude with respect to the range 


and P,(—) is the total variance of the altitude. Thus, R, can be written as 


0 
R, = a, = (——)’0?,,, (3.6) 


Orange 


where o..,, is the variance associated with each scan of the optical radar scanner. 
In the case of P,(—), there is not only a variance in the z direction but there is also 


a variance in the x and y calculations due to the noisy range values. Thus, P,(-) 


can be written as 


Q Oz 
Py(=) = oma = 07 + (—)'o? + (—)’0? | (3.7) 
Oz Oy 


Using Eqs. (3.6) and (3.7), Eqs. (2.20) and (2.21) can be written as 


K, - o stotal(—) eo 7 a) (3.8) 


and 


Py(+) = [2;%rtoal—Dle; + Pxoia(—) - (3.9) 

2. Compensating for Walker Motion 
When the ASV is walking, or when there is an INS initialization error, 
the problem becomes one of computing the optimal value for the altitude and 


ensuring it is stored in the correct terrain cell. Due to the inaccuracies of the 
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altimeter and GPS inputs discussed in Section C of Chapter II, it 1s necessary to 
estimate the difference in the horizontal coordinates, Az and Ay. For this part of 
the problem, regression analysis will be used to select the correct terrain cell. 
Then the optimal altitude value calculated from Kalman filtering will be stored. 
Two methods of regression analysis will be discussed for use in this problem: the 
gradient descent method and a grid search method. 
a. Gradient Descent Method 

In this approach, there is a criterion function, ¢, for a given set of 
parameters. The criterion function will be the sum squared error between the 
range returned by the optical radar scanner, R, and the range computed from the 
internal terrain map pre-stored in the ASV computers, R. From this, Eq. (2.3) 


can be written as 
Gp Rian ah ek) (3.10) 


To correct for the x and y coordinate drift, the gradient of 6, v@, and 
the Hessian matrix, H, will be estimated using a 3 x 3 grid around the INS values 
of the x and y coordinates. Using the procedure described by Poulus in Ref. 10, 
the 3 x 3 terrain cell mask is set up with the cell of interest at point where x=0 
and y=O with its associated value of 6,. Table 3 shows the relative distances of 
the cells and the criterion functions associated with them. 

A quadratic function can be fitted to the cells of interest as a function 


of x and y which has the form 
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TABLE 3 
RELATIVE DISTANCES OF TERRAIN 
CELL MASK AND CRITERION FUNCTIONS 





R= Rana? + ke + kr + kezy + key : 
Eq. (3.11) can be written as 
R= Age Kees 


where the subscripts are the dimensions of the matrices and 


Z 
k = (k, ky ky ky ks ke) 


and 


A; = (1 zi OY; z, zy; y,) i 


(3.11) 


(3.12) 


(3.13) 


(3.14) 


By substituting Eqs. (3.12-3.14) into Eq. (3.10) and taking the partial derivatives 


with respect to the k,’s, Eq. (2.4) becomes 
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a 
— = = -2A'6" + 2A7 Ak =0 (3.15) 
Ok. 


or 
A" Ak = A’®’ . (3.16) 
Rearranging terms, Eq. (2.6) can be written as 
k=[A7A]'A’O” . (3.17) 
The gradient of the terrain cell of interest is [Ref. 10: pg. 59| 
V® = kot + ky (3.18) 
and the Hessian matrix is [Ref. 10: pg.60] 


k, 2kg 


H = . (3.19) 
2k, &; 


Applying Eqs. (3.18) and (3.19), the optimum amount of Az and Ay 


can be computed as 
(Az,Ay) = H v® (3.26) 
which leads to the best estimate for the x and y coordinates of the terrain cell as 
(2,9) optimum = (Xiws + Az, Ving + Ay) (3.21) 


where X,,, and Y,,, are the coordinates supplied by the INS system. 
In order to be able to compute the optimum values of Az and Ay, the 
values of the k’s in Eq. (3.17) have to be computed. Equation (3.17) can be re- 


written as 
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k=[A’A] ‘A’? = Bo (3.22) 


where 
B=|A'A]'A’™ . (3.23) 
To avoid repeated lengthy matrix computations, Ref. 10 shows B to be 
-1/9 2/9 -1/9 2/9 5/9 2/9 -1/9 2/9 -1/9 
-1/6 0 1/6 -1/6 0 1/6 -1/6 0 1/6 
1/6 1/6 1/6 0 0 O -1/6 -1/6 -1/6 
B= . (3.24) 
io -1/3' 1/6 1/6 =1/8 1/6 1/6 -1/8 1/6 
-1/4 O 1/4 0 0 0 1/4 O -1/4 
1/6 1/6 1/6 1/3 -1/3 -1/3 1/6 1/6 1/6 
b. Grid Search Method 
For this search method, the assumption is made that the criterion 
function, , is definitely reduced when a sufficiently small step is taken in the 
direction of the actual terrain cell. Applying this to the terrain cell mask in Table 
3, the distances between each cell will be assumed to start at one standard 
deviation, o, of the INS system estimate of x and y instead of a unit 
displacement. After each computation of the #’s in the 3 x 3 mask, the least 
value # is moved to the center of the mask and the coordinates of that terrain cell 
become the terrain cell of interest and the 3 x 3 mask and the associated ©’s are 
again calculated with each cell being displaced by o. Once the least value of 6 


remains in the center cell for two successive searches, then the displacement of 
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each cell, A, becomes 





jo n=1,2,3 . (3.25) 


The advantage of this type of search is that it does not depend on the 
cell of interest and its surrounding neighbors being able to be fitted with a 


quadratic function and thus can be applied to a larger variety of terrains. 


E. SUMMARY 

This chapter has presented the necessity for an accurate terrain altitude map 
before a vehicle can become autonomous. After the data from an optical radar 
scanner is converted into a form that can be used for route planning, there are 
various ways to compute the optimal altitude values and store them in the proper 
terrain cell. One method for computing the optimal altitude values was presented 
along with two ways to determine the proper terrain cell to store the altitudes. In 
the next chapter, the simulation model is presented that will be used to test these 


data manipulation methods. 
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IV. EXPLANATION AND JUSTIFICATION OF SIMULATION MODELS 


A. INTRODUCTION 

In Chapters II and III, various ways of handling data with INS and optical 
scanner errors were discussed. In order for these schemes to be tested, it is 
necessary to make use of either the ASV or similar vehicle, or an accurate 
computer simulation. The obvious choice for this thesis is the computer. In this 
chapter, a simulation model is presented that will allow simulation of terrain 
scanning by the optical-scanner and then data manipulation using the schemes 


presented earlier. 


B. TERRAIN AND OPTICAL SCANNER MODELS 

The computer chosen for the simulation studies of this thesis is an ISI 
Optimum V Workstation. This is a graphics workstation with an UNIX4.2BSD 
operating system. The workstation has a high-resolution display for a two- 
dimensional, black and white graphics display. The display is 1280 pixels wide 
and 1024 pixels high with the upper left corner of the screen functioning as the 
reference point. The operating system has installed libraries of graphics routines 
which are accessed by the computer language C, which is the language chosen for 
this thesis. These libraries provide graphics tools which make it easy to run a 
visual simulation. [Ref. 13] 
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In order to simulate the operation of the optical scanner, a simulated terrain 
is needed. The method for generating and drawing this terrain is the same as 
that presented in Ref. 10. An elevation oblique projection is used in this thesis to 
display a three-dimensional model of the terrain [Ref. 14: pp.272-316]. In the 
elevation oblique representation, lengths and angles of lines in a vertical plane are 
preserved while others are distorted. Figure 2 shows the coordinate systems used 
for the Cartesian coordinate system, the ISI screen coordinate system, and the 


image coordinate system. Using these coordinate systems, the following equations 


apply: 
u=2z— .5y (4.1) 
v=—z— By (4.2) 
X = Xiigin + 4 (4.3) 
Y = Youn — 2 (4.4) 


The use of the multiplier value of 1 in Eqs. (4.1) and (4.2) means that each pixel 
in x and z corresponds to one inch in physical units. The use of a scale factor of 
.5 for the y coordinate in both of the equations means that y is foreshortened by a 
factor of .707 relative to the scale of x and z. With these scale factors, the terrain 
shown in Figure 3 is of dimension 64 ft. x 64 ft. With Eqs. (4.3) and (4.4), the 


reference of the terrain is set in the lower left corner of the terrain. 
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(c) 


Figure 2 
Coordinate Systems 
a) Cartesian , b) Screen , c) Image 
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Figure 3 
Simulated Terrain 
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The terrain data is generated by 


z=r4 eae He). 2x r3) (4.5) 
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where r3 and r4 are random numbers and x and y are the horizontal coordinates 
of the altitude map measured in inches. Figure 3 is a picture of typical terrain 
generated by Eq. (4.5). 

Figure 4 is a flow chart for the simulation program. The computer code in 
Appendix A is commented for further explanation of each segment of code. 
Figure 5 is a flow chart of the simulation of the optical scanner scanning the 
terrain. In scanning the terrain, the optical scanner’s beam length is increased 
one pixel (one inch) at a time. After each increase of the beam, the x, y, and z 
coordinates of the end of the beam are calculated using the D-H transformation 
discussed earlier. The calculated value of z is then compared to the value of the 
altitude stored at the x and y coordinate of the actual terrain data. If the 
calculated value of z is greater than the actual terrain value, the beam length is 
increased and the process starts again. If the calculated value is less than the 


actual terrain value, then the optical scanner’s range is set to 


R= —..5 (4.6) 


beam 


where R is the range to the z value that is less than the actual terrain value 


beam 
and the .5 adjustment brings the range back to a value closer to the actual range. 
The actual ERIM optical scanner scans the terrain in 128 increments, both in 


elevation and azimuth. However, due to the time requirements of simulating the 
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Figure 4 
Simulation Flow Chart 
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Figure 5 
Scanning Flow Chart 
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scanning process, the simulation of this thesis uses twelve increments of one 
degree each in both the azimuth and elevation. 

It is assumed that the error associated with the the optical scanner is range 
dependent; that is, the greater the range to the surface, the greater the error of 
the returned range value. For the purposes of this thesis, this error will be 
represented by 


Pa a5 7 o,R (4.7) 


scan 


where o..,, is the total variance of the scan and R is the range from Eq. (4.6). 


scan 


Also, lacking any information about the scanner errors, a, and o, are set at 
o, = 02 in’ (4.8) 
and 
o, = 04 . (4.9) 
After the variance in the range measurement is computed, the new value of 
the range, R, is computed by 


R=R+no (4.10) 


ecan 


where n is the output of a gaussian random noise generator with zero mean and 


unit variance and 


OC ucan V OC scan * (4.11) 
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C. DATA HANDLING DURING SIMULATION 

Once the terrain has been scanned by the optical scanner, regression analysis 
is applied to compute any errors in the INS values for the x and y coordinates. 
Figure 6 is a flow chart of the gradient search method of regression analysis. 
Using the INS values of the x and y coordinates, ®, is computed using Eq. (3.10). 
The INS value of the x and y coordinates are then changed by the values in Table 
3 and the associated 6’s are computed. To compute the ’s, the terrain 1s 
scanned, but this time without noise added to the range value. In the actual ASV, 
this would be done internally with the pre-stored terrain map of the area and not 
an actual scan of the terrain. From these values of @, Eqs. (3.22) and (3.19) are 
used to compute the k’s and the H matrix. With these, Az and Ay are 
determined by Eq. (3.20). With these values of Az and Ay, the INS values for x 
and y can be adjusted by Eq. (3.21). 

The second regression analysis method is the grid search method. Figure 7 is 
the flow chart for this search method. It is much the same as the gradient search 
method except that the INS values of the x and y coordinates are changed by the 
standard deviation, o, of the INS estimate of x and y. After each cell’s © is 
computed, the minimum value of © is determined, and if it is not in the center 
cell, then it is moved to the center along with its associated value of x and y. If 
the minimum @® is already in the center cell, it is left there. If the minimum © 
was not in the center cell, then after it is moved to the center, the x and y values 


are again changed by o. This process continues until the minimum 6 stays in the 
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center cell for two successive iterations. Once this condition is satisfied, the x and 
y values are changed by a/2” and the 6’s are again computed. This process 
continues for n=1,2,3. Upon completion of the search method, the cell with the 
minimum @ has the new INS value of x and y to replace the old values. 

After completion of the regression analysis, the Kalman filter is used to store 
the optimal values of the altitude in the appropriate altitude map cell. The cell 
indices are determined using the new INS values for x and y determined by 
regression analysis. Figure 8 is the flow chart for the Kalman filtering. For 
initialization of the Kalman filter, if a cell in the altitude map does not have a 
stored value for 0°14, then the go’, of that cell is set to be (120)’, which is the square 
of the vehicle’s height. From Eq. (3.7), the partial derivatives with respect to x, 
y, and z are dependent on the nature of the terrain. Eqs. (4.12) and (4.13) show 


that o, and a are given by 


Oz 
2 Z 2 
a? = 03}(—) 4.12 
i (4.12) 
dy 
2 2 Z 
Fd 1 (4.13) 


For purposes of this thesis, the values of «7 and oy are set to 1, but in reality, they 
are dependent on range as well as azimuth and elevation scan angles. 


From the terrain data generated for this simulation, an average value for 


Oz 
(—)’ was determined from the difference in the altitude value in the x direction 
z 
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Figure 7 
Grid Search Flow Chart 
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with the y value held constant. From these calculations, the average value of this 


quantity is 
Oz » ae 
(~) = .116in . (4.14) 


Oz » ; OZ 
The average value for (—) was calculated in the same manner as that for (—) 
Oy Oz 


with the results the same as those in Eq. (4.14). Next, a? is set to be 
a = sin’O, Bits (4.15) 
With the value of o;,,,,, computed, Eqs. (3.5) and (3.9) are applied to each cell of 


the altitude map to store the optimal value of the altitude. Upon completion of 


the filtering, the program returns to scanning the terrain. 


D. SUMMARY 

In this chapter, the computer simulation was discussed. The creation and 
scanning of the simulated terrain and the scanning of it was presented along with 
the two regression analysis schemes and the Kalman filtering scheme for handling 
the data obtained from the scanning. In the next chapter, the results of the 


simulation runs will be discussed. 
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V. SIMULATION PERFORMANCE 


A. INTRODUCTION 

In the previous chapters, methods for correcting INS drift errors were 
presented that might contribute to an effort to make the ASV an autonomous 
vehicle. After development of a computer simulation model in Chapter IV, 
humerous simulation runs were needed to validate not only the model but the 
various schemes for handling data so that an accurate terrain altitude map can be 
developed for future use in possible route planning. In this chapter, simulation 
runs are planned and run for a stationary walker and a walker with movement 
involved. Along with the results, some conclusions about these results are 


presented. 


B. SIMULATION RESULTS 
1. Stationary Walker Results 

The first test of this thesis was to run the simulation using a stationary 
ASV. In this case, the walker is in a permanent position and it scans the same 
terrain over and over while performing Kalman filtering on the altitude 
information. Figure 9 shows the simulated terrain with the area being scanned 
indicated on it. In this figure, the optical scanner is represented by a small square 
and there is a line drawn from the scanner down to the terrain to give the viewer 


ol 


an idea of the scanner’s relative position over the terrain. Also, the INS origin is 
set to be the lower left hand corner of the terrain. Due to the assumptions made 
in Chapter III, Section C, the x coordinate increases positive to the right from the 
INS origin along the horizontal axis, and the y coordinate increases negative along 
the other (diagonal) axis. 

To evaluate the effectiveness of the Kalman filtering presented in this 
thesis, three terrain cells are looked at to see how the optimal altitude value 
changes as the terrain is scanned in a stationary position. To get a representation 
of how the filtering is working at different ranges, the three terrain cells are 
selected at scanner elevations of -26, -41, and -61 degrees. Figure 10 is a plot of 
the three terrain cells altitudes stored after each scan after Kalman filtering has 
been applied. To check that the filtering works no matter what the noisy range 
values returned by the optical scanner are, a second simulation was run on the 
same terrain in the same position. Figure 11 shows the results obtained for the 
same three terrain cells. 

The next test made on the stationary walker was done with the scanner 
in a different position on the terrain and pointing in a different direction. Figure 
12 shows the scanned terrain for this case. Figure 13 shows the results of the 
Kalman filtering on three terrain cells at the same elevation angles used in the 
first case. Figure 14 shows the same three terrain cells with different noisy optical 


scanner range values. 
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Figure 9 
Scanned Terrain ] 
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Figure 10 
Kalman Filtering Results, Run 1 
Stationary Walker on Terrain 1 
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Figure 12 
Scanned Terrain 2 
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Kalman Filtering Results, Run 1 
Stationary Walker on Terrain 2 
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The last simulation conducted for the stationary walker case was to scan 
the same terrain shown in Figure 12 and let the scanner scan it for 40 scans. 
Figure 15 shows the results of the 40 scans for the same terrain cells looked at in 
Figure 13. 

2. Moving Walker Case 

The next case to look at was the walker with motion involved. To get 
the optimal altitude value for each terrain cell, errors in the INS values of the x 
and y coordinates must be corrected for so the altitude value can be stored in the 
correct cell. For the purposes of this thesis, the net effects of walker motion are 
represented by an INS initialization error or offset. 

The first method used to correct for the INS errors was the gradient 
descent method. The scanner was placed in the same position as represented in 
Figure 9 and given an initialization error of Az = 5 inches and Ay = —4 inches. After 
the first scan, gradient descent was applied using Eq. (3.20), and the optimum 
values for Az and Ay were 10 and 5 respectively. A second run was conducted 
with different INS errors and the results were again not close to the value they 
should have been. The reasons for this failure are not known, but appear to be 
associated with the inadequacy of a quadratic approximation to the squared error 
function, . This problem might be solved by a more elaborate gradient descent 
method as discussed in Ref. 8, but this approach was not investigated in this 


thesis. 
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After the apparent failure of the gradient descent method, the grid search 
method was applied to the moving walker case. The first simulation run using 
the grid search method was with the scanner in the position indicated in Figure 9 
and with the standard deviation of the INS offset given by, o =6 inches. This a 
was an assumption and is used in the terrain cell mask described in Chapter II, 
Section D. To test the effectiveness of the grid search method, a run was 
conducted with the INS error less than o, another run with the error between o 
and 20, and a third run with the error greater than 2c. Figure 16a shows the 
results of the regression analysis with the INS error less than o. The plot shows 
the actual x and y values, the starting point of the INS x and y coordinates and 
the numbers indicate the steps of the grid search. The final value is the value 
returned to be used in the Kalman filtering by the grid search at the end of the 
first actual terrain scan. As Figure 4 shows, the regression analysis is repeated 
each time a new frame of noisy range values is obtained from the simulation. 
Figure 16b shows the results of the grid search after the first scan with the INS 
error between o and 2c. After the second scan, the data showed that the grid 
search method locked in on the actual x and y coordinates and stayed there for 
subsequent scans. Figure 17 shows the results of the grid search after the first 
scan with the INS error greater than 2c. The data collected also shows that the 
grid search method would not lock in on the actual x and y values on the 


succeeding scans when the error was greater than 2c. 
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Upon completion of each grid search, Kalman filtering was applied to 
obtain the optimal altitude value for each terrain cell. Figure 18 are the results of 
the Kalman filtering after 8 scans with the INS error less than o. The data also 
shows that the results of the Kalman filtering when the INS error is between oa 
and 20 are almost exactly the same as those shown in Figure 18. Figure 19 
presents the results of the Kalman filtering when the INS error is greater than 2c. 

The next test of the grid search method was to set the value of 
o = 12 inches. With this value of o, the same type of runs were conducted as when 
o = 6inches. Figure 20 shows the results when the INS error is less than 2c. Figure 
20a shows the grid search locks in on the actual x and y values with zero error 
after the first scan. Figure 20b indicates that the grid search did not lock in on 
the actual values after the first scan. To further test the grid search with the INS 
error between o and 20, another run was conducted with different errors in that 
range and the results are shown in Figure 21. The data showed that the grid 
search locked in with zero error on the actual position on the first scan and stayed 
locked in on all succeeding scans. The final run was conducted with the INS error 
greater than 2o and those results are shown in Figure 22. As with the results 
when o = 6 inches, the grid search would not lock in on the actual position when 
the INS error was greater than 20. When the Kalman filtering data was reviewed, 
the results were very much like that received when o = 6 inches. 

So far all of the results for the moving walker case involved using a pre- 


stored terrain altitude map. One final simulation conducted was to see if the grid 
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Figure 20 
Grid Search Results With o = 12 inches 
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search method would work without a pre-stored map. In this case, the scanner 
was placed in the position represented in Figure 9 and the terrain was scanned 20 
times to obtain an altitude map. From previous results, it is known that the map 
stored for the area scanned was very close to the actual map and the data for this 
case also showed that to be true. Upon completion of the 20 scans, the scanner 
was given an offset and then regression analysis and Kalman filtering was applied 
for the next 8 scans. Figure 23 shows the results of the grid search after the first 
scan. As can be seen, the grid search went farther away from the actual position. 
Actually, the grid search only conducted 3 steps because of the safety factor built 
into the program so it would not look forever if it was not converging. Also, the 
data indicated that on the succeeding scans, the grid search actually got worse. 
This run was conducted with an o = 6 inches and the offset between o and 20. It 


was run again with the offsets less than o and the results were the same. 


C. CONCLUSIONS 

The first general conclusion to be made from the results presented in this 
chapter is that the Kalman filtering is working. As can be seen from the results of 
the stationary walker case, the stored altitude values were very close to the actual 
values. For the terrain cells where the scanner was at -26 and -61 degrees, the 
stored values were within .25 inch of the actual values while the one at -41 
degrees was within about 1.1 inches. To evaluate the significance of these results, 


data was obtained concerning the standard deviation of the raw terrain elevation 
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values obtained from the noisy range values. The observed standard deviation 
had a value of 2.95 inches when averaged over all cases, much greater than the 
filtered values. One possible reason why the stored values at -41 degrees had a 
greater error was the arbitrary range adjustment from Eq. (4.6) and the sin’ effect 
of Eq. (4.15). In any case, these results would probably be good enough for some 
route planning program to use. 

Referring to Figure 15, an apparent bias in terrain altitude estimates is 
revealed. From the magnitude and general behavior of this bias, it is believed 
that it results from the quantization of range into increments of one inch. This 
quantization is modulated by the sin’ term of Eq. (4.15), leading to the differences 
between the three curves of Figure 15. 

The next conclusion is that the gradient descent method of regression analysis 
was not effective with this simulated terrain. The reason for this failure is not 
known except it may have something to due with the terrain and the resulting 
"lumpy" error criterion function, 9%. 

The overall conclusion about the grid search method is that it is effective. 
How well it works depends on the o selected and the amount of INS error relative 
to that o. As Figure 20b showed, with a INS error of Az = 21 inches and 
Ay = 19 inches, the grid search would not lock in on the actual position but as seen 
in Figure 21, with an error of Az = 18 inches and Ay = —17 inches, the grid search 
locked in on the actual position. This would indicate that there might be a limit 


to the amount of INS error involved before the grid search can correct for the 
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error which is independent of the value of o. With this method though, the INS 
initialization errors or offset can be corrected for, within limits, and thus allowing 
the Kalman filtering to provide an accurate terrain altitude map when a pre- 
stored altitude map is available. 

Finally, using the grid search method without a pre-stored altitude map did 
not work. In analyzing the results, a possible reason for its failure was because 
the altitudes in all of the surrounding terrain cells was initialized to zero so when 
the terrain mask was applied and the stored map scanned, the altitude values 
stored in the resulting Cartesian coordinate map would probably be less than they 
should have been, thus giving a minimum criterion function in a false cell during 
regression analysis. With this false minimum, the grid search would chase after 


the wrong cell. 
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VI. SUMMARY AND CONCLUSIONS 


A. RESEARCH CONTRIBUTIONS 

In this thesis, a first approach was presented in providing an optimal terrain 
altitude map that could be used for route planning for an autonomous walking 
machine. Toward that end, an effective Kalman filtering scheme was developed 
and tested. The results of those tests show that the altitude values stored in the 
terrain map are good enough for a walking machine to walk across. 

Another important part of providing an optimal altitude map is to ensure 
that the altitude values are stored in the right terrain cell locations associated 
with the proper x and y coordinates of the terrain area being traversed. If the x 
and y coordinates are in error because of errors in the inertial navigation system, 
then it is necessary to correct for those errors before storing the output of the 
Kalman filter. To solve this problem, a regression analysis approached called the 
grid search method was presented. This search method proved to be effective in 
correcting for these land navigation errors on the simulated terrain within limits. 
These limits appear to be within about 18 inches of the actual x and y coordinate 
values with the terrain used in this thesis. For a first approach, this type of 
accuracy is good and provides for a satisfactory starting point for refining this 


method. 
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Another contribution of this thesis was to provide a simulation for terrain and 
an optical terrain scanner scanning that terrain. This simulation was developed 
for the ISI graphics workstations and could be helpful for any further work that is 


conducted on these or similar workstations. 


B. RESEARCH EXTENSIONS 

Since the topics presented in this thesis represent a first attempt at optical 
radar data filtering with the ASV in mind, there are many areas that can be 
expanded upon. 

First, further research is needed into why the gradient descent method did not 
work. It is very possible that the reason has something to due with the simulated 
terrain used. Another extension that could be pursued would be with the grid 
search method. Along with having a decreasing mask in the grid search, an 
expanding mask could be developed if the minimum criterion function cannot be 
brought into the center of the grid. Also, further research is needed to determine 
the optimal value for the grid size to use in the mask and what limits there are on 
the errors that can be corrected. Along with this, the grid search method should 
be applied to completely different terrain and the optimal value of grid size 
determined and then compared to the one for this terrain to see if there is a 
relationship. Next, investigation into ways to use the grid search method to 
correct for errors introduced by the accelerometers while walking would allow the 


vehicle to continuously walk while the grid search method corrects its position. 
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Currently, under the scheme presented in this thesis, the vehicle would have to 
take a few steps, stop and correct its position, and then take a few more steps. 

Another important area that needs to be studied is the one of applying these 
techniques to the moving walker case where no pre-stored altitude map is 
available. As seen in Chapter V, the first attempt involving this situation did not 
work. Research into why it did not work and how to make it work will be 
invaluable to making the walking machine autonomous. 

The last area that should be studied is to apply the techniques that have 


proven successful in simulation to actual terrain with actual optical scanner data. 


76 


~] 
e 


10. 


LIST OF REFERENCES 


McGhee, R. B., and Waldron, K. J., An Ezpertmental Study of an Ultra- 
Mobile Vehicle for Off-Road Transportation, Final Technical Report for 
DARPA Contract MDA903-82-0058, College of Engineering, Ohio State 
University, Columbus, Ohio, 1984. 


Raphael, B., The Thinking Computer-Mind Inside Matter, W. H. Freeman, 
1976. 


Flynn, A. M., Redundant Sensors for Mobile Robot Navigation, M.S. Thesis, 
Massachusetts Institute of Technology, September 1985. 


Klein, C. A., Olson, K. W., and Pugh, D. R., "Use of Force and Altitude 
Sensors for Locomotion of a Legged Vehicle Over Irregular Terrain", The 
International Journal of Robotics Research, vol. 2, no. 2, Summer 1983. 


Lowrie, J.. The Autonomous Land Vehicle Program Report, Martin Marietta 
Denver Aerospace, Denver, Colorado, December 1985. 


Nitao, J. J., and Pardi, A. M., "A Real-Time Reflextive Pilot for an 
Autonomous Land Vehicle", JEEE Control Systems Magazine, February 
1986. 


Zuk, D.,Pont, F., Franklin, R., and Dell’Eva, M., A System for Autonomous 
Land Navigation ,Technical Report , Environmental Research Institute of 
Michigan, Ann Arbor, Michigan, November 1985. 


McGhee, R. B.. Identification of Nonlinear Dynamic Systems by Regression 
Analysis Methods, Ph.D. Dissertation, University of Southern California, Los 
Angeles, California, June 1963. 


Gelb, A., Applied Optimal Estimation, The MIT Press, Cambridge, 
Massachusetts, 1986. 


Poulus, D., Range Image Processing for Local Navigation of an Autonomous 
Land Vehicle, M.S. Thesis, Naval Postgraduate School, Monterey, California, 
September 1986. 


77 


Ph 


12. 


13. 


14. 


Richbourg, R., Solving a Class of Spatial Reasoning Problems: Minimal-Cost 
Path Planning in the Cartesian Plane, Ph.D. Dissertation, Naval 
Postgraduate School, Monterey, California, June 1987. 


Fu, K. S., Gonzales, R. C., and Lee, C. 8S. G., Robotics: Control, Sensing, 
Viston and Intelligence, McGraw Hill, 1987. 


Anon, Programmers Reference Manual for Graphics Software, Integrated 
Solutions Inc., San Jose, California, September 1985. 


Foley, J. D., and Van Dam, A., Fundamentals of Interactive Computer 
Graphics , Addison-Wesley Publishing Company, 1984. 


78 


APPENDIX 


PROGRAM LISTING 


ee i ee en tS TSR TERS SAS EHS 


This is a computer simulation of an optical 
scanner scanning simulated terrain using 
regression anaylsis and Kalman filtering. 
This program is written in C for the ISI 
graphics workstations. 

This program is compiled by 


cc scan.c -ltools -lbm -lvt -lm 


by Mark D. Rickenbach 
1 August 1987 


EMneGMe oe TTS SPA SES ELSES ERE S LSS / 


#include <math.h> 

#include <vt.h> 

#include <stdio.h> 

#include <bitmap.h> 

#include <tools.h> 

struct BMD *bmd; 

int seedl; 

float seed,seed2,a0 9/4]|[4],seed3; 

main() 

{ 

static int u,v,u2,v2,scan,li,jj,v21,u21,rr,f,init ,x,y,z,k; 

static int randl,rand2,rand3,rand4,xint,yint,safe,counter; 
static int bmyl,bmx1,bmx,bmy,fd,i,j,kk,xx,yy,s,rang,]; 

static float sigscan,sig02,sig12,array|128]/128],d9,r/456];: 

static float n,sigzold2/128][128],noise(),sigscan2,elev,dx(9],dy(9]; 
static float xxl,yy1,zz1,d3min,gmin,zhat[128]/128],zz/128][128]; 
static float w,tl,celev,melev,sigold2/128]/128],sigz2/128]{128]; 
static float th4,th5,th6,d1,d2,d3,th7,th8,g[8],rhat/456],d2min; 


an ct a: cada dl lad cc cadaaaliadiaad tii i led a7 alata 


Open window, allocate bit map and set line discipline to graphics 


for graphics simulation ASV scanning terrain. 
eg 5 8 TEA AL SEES SSS SS SSL SESE RASS E RS REE ER REL / 


fd=Open Window(15,63,1220,664,"Terrain Map"); 
Set LineDise(fd,TWSDISC); 

if ((bmd=BM_ Allocate(1220,664))==0) 

{ printf(" unable to allocate bit map"); 


BM_SetAddressing(bmd,1); 
BM_SetColor(bmd,1); 
BM_SetBColor(bmd,0); 
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[ RAE SSEREAR ERS ER EEE ES ESSERE OS eee eae) aera 


Initialize various arrays to zero. 
FER EERA SEER EES EE ea aida ee ener 


for(i=0;i1<128;++i) 

{for(j=0;j<128;+-+4)) 
{sigold2{i]|j]=0; 
zz{i][j]=0; 
mapz(il(j]=0; 
sigz2(i]{j]=0; 


} 


[RE Ee Ee aaa ena al 


Compute array of altitude values for terrain map. 
Fee ee ee ee eee ea eel ere eget eek 


counter=0; 


do { 
seed=0.0; 
seed 1=0; 
seed2=0.0; 


rand1=ran(21); 
rand2=ran(21); 
rand3=ran(1); 
rand4=ran(121); 
for(i=0;1<128;++1) ; 
{ for(j=0;j<128;++)) 

{x=rand1-i; 

y=rand2-); 

t1=(x*x) + (y*y); 

w=(sqrt(t1)/30.)*6.2832*rand3; 

array |j|{i]=rand4*cos(w); 


} 


counter=counter + 1; 
} while (counter < 0); 


[ REREEREER ESLER LEDERER LEAS Se ee a 


Draw the base of the terrain map. 
See eee eee ne ee en ee la eee ne 


xint=30; 

yint=54?2; 
BM_SetPosition(bmd,xint,yint); 
BM_SetThickness(bmd,3); 

BM _PaintLine(bmd,792,542); 
BM _PaintLine(bmd,1176,161); 
BM _ PaintLine(bmd,414,161); 
BM _PaintLine(bmd,xint,yint); 
BM_SetThickness(bmd,1); 
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ais aaa i ial si iad iad dadaliadailadadiadadialaiialataaiaiaiaiaiaiial 


Draw terrain to bit map using scheme developed by 
Poulus in his thesis. Each terrain cell will be 


represented by a 6 in. x 6 in. square. 
nnn ema gn tT TT ETERS SESS ELSES SEES SESE ESSS | 


i=0; 

j=0; 

loop: 

x=i"6; 

y=}"6; 

z=array|j|(il; 

bmx=xtrfunc(&xint ,&x,&y); 

bmy=ytrfunc(&yint, &y,&z); 

loop]: 

iG@==127 && j=——127) 

{ BM_SetThickness(bmd,2); 

bmy l=bmy-+4z; 
BM PaintLine(bmd,bmx,bmy1); 
BM _ SetPosition(bmd,bmx,bmy); 
BM _SetThickness(bmd,1); 
BM _DisplayBitmap(fd, PAINTRASTER,bmd,0,0,0,0,1220,664,0); 
goto loop2; : 


} 
ifg—=127) 

{if('==0) 

{ x=0; 
y=)*6; 
bmx=xtrfunc(&xint,&x,&y); 
bmy=ytrfunc(&yint,&y,&z); 
BM _ SetPosition(bmd,bmx,bmy); 


} 

BM_SetThickness(bmd,2); 
bmyl=bmy-+z; 

BM PaintLine(bmd,bmx,bmy]); 
BM _SetPosition(bmd,bmx,bmy); 
BM _ SetThickness(bmd,1); 
i=1+1; 

mei "S; 

y=)*6; 

z=array (j](i); 
bmx=xtrfune(&xint,&x,&y); 
bmy=ytrfune(&yint,&y,&z); 

BM PaintLine(bmd,bmx,bmy); 
goto loop]; 
} 

if(i==127) 
{ BM SetThickness(bmd,2); 

bmyl=bmy-+z; 

BM PaintLine(bmd,bmx,bmy1); 
BM _ SetPosition(bmd,bmx,bmy); 
BM _SetThickness(bmd,1); 
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ileal 
y=}"6; 
x=i"6; 
z=array |j][i]; 
bmx1=xtrfunc(&xint ,&x,&y); 
bmy1=ytrfunc(&yint,&y,&z); 
BM_PaintLine(bmd,bmx1,bmy1); 
i=0; 
goto loop; 
} 
if{i==0 || j==0) 
{ if(i==0 && j==0) 
{BM SetThickness(bmd,2); 
BM _PaintLine(bmd,bmx,bmy); 
goto loop4; 
} 
x=1"6; 
y=}"6; 
z=array|j]|i]; 
bmx=xtrfunc(&xint,&x,&y); 
bmy=ytrfunc(&yint,&y,&z); 
BM SetPosition(bmd,bmx,bmy); 
BM SetThickness(bmd, 2); 
bmyl=bmy-+z; 
BM_PaintLine(bmd,bmx,bmy1); 
loop4: 
BM_SetPosition(bmd,bmx,bmy); 
BM _SetThickness(bmd,1); 
i=i+1; 
x=(i-1)*6; 
y=(j+1)*6; 
z=array|j+1][i-1]; 
bmx1=xtrfunc(&xint,&x,&y); 
bmy1=ytrfunc(&yint,&y,&z); 
BM PaintLine(bmd,bmx1,bmy1); 
BM SetPosition(bmd,bmx,bmy); 
X=156; 
y=)"6; 
z=array(j]|i]; 
bmx=xtrfunc(&xint,&x, &y); 
bmy=ytrfunc(&yint,&y,&z); 
BM _PaintLine(bmd,bmx, bmy); 
goto loop]; 
} 
i=1+1; 
x=(i-1)*6; 
y=(j+1)*6; 
z=array|j+1]|i-1]; 
bmx1=xtrfunc(&xint,&x,&y); 
bmyl=ytrfunc(&yint,&y,&z); 
BM PaintLine(bmd,bmx1,bmy1); 
BM SetPosition(bmd,bmx,bmy); 
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x= 170: 
y=}*6; 
z=array|j)|i]; 
bmx=xtrfunc(&xint,&x,&y); 
bmy=ytrfunc(&yint,&y,&z); 
BM PaintLine(bmd,bmx,bmy); 
goto loop]; 

loop?2: 


eeIR rs eS SEAS NESSES SSE EES PES CEES ES AF 


Enter the starting LANS information from keyboard. 
i sees et eT SS ES EEA SSSEESE SSAA SL OSES 


printf(" enter x-coord. of ASV 0); 

scanf("%f" &d2); 

printf(" enter y-coord. of ASV 0); 

scanf("%f" &d3); 

printf(" enter z-coord. of ASV 0); 

scanf("%f" &d1); 

printf(" enter scanner body azmuith angle 0); 
scanf("%f" ,&t4); 

printf(" enter scanner body elevation angle 0); 
scanf("%f" &t5); - 

printf(" enter scanner body roll angle 0); 
scanf("%f" &t6); 

scan=0; 

ox 1—dz- 

yy1=d3; 

zz1=dl. 

d2=d2+29; /* Introduce LANS initialization errors if any */ 
d3=d3-30; 


en ee eee TEASE EES EASES ALES ETERS 


Start scanning terrain. 
en een SESE SESE LESSEE SESE EELS SS | 


do { 
scan=scan+]; 
printf(" scan %d ",scan); 
u2=(xx1-0.5*yy1+30); 
v2=(221+.5* yy1+542); 


ee RRR SEF SAAR ELAS ES SEES ASSN EEE SAAR ESE ASSES SE 


Draw a rectangle to indicate position of the ASV scanner. 
ee eee SEE LESSEE SSSA ESTAS SESE TEL TREE SEES ERS / 


BM _SetPosition(bmd,u2,v2); 

BM_SetAddressing(bmd,0); 

BM _PaintRectanglelnterior(bmd,8,8); 
BM_DisplayBitmap(fd,PAINTRASTER,bmd,0,0,0,0,1220,664,0); 
BM_SetAddressing(bmd,1); 

BM_SetThickness(bmd,2); 
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u21=xx1-.5*yy1+30; 
v21=.5*yy1+542; 
BM_PaintLine(bmd,u21,v21); 
BM_SetPosition(bmd,u2,v2); 
BM_SetThickness(bmd, 1); 


Dee ee ee ery cea te elcome eee oe eae ae a aes 


Scan terrain with one full azmuith scan for each elevation 
increment starting at elevation representd by elev and 
scanning down. For init=1, this simulates scanner operation 
returning a noisy range value, r(rr]. For init=2, this 
simulates the ASV’s computer internally scanning its 
pre-stored terrain map and returning noiseless range values, 
rhat|rr|. For the purposes of this thesis, the scanner 

scans in three elevation segments of 12 degrees each at 

-20, -35, and -55 degrees. 


SEER Ee TERE EE Re eee Re tee eee 


for(init=1;init<3;init++) 
{rr=0; 
i=1i; 
do{ . 
ii=—1) 
{elev=(-1)*20.; 


else if (i==13) 
{elev=(-1)*22.; 


else if (i==25) 
{elev=(-1)*30; 


th7=elev-i; 
tt=1; 
j=1; 
do { 
rr=rr+1; 
th8=6-}; 
if(tt==1) 
{rang=0; 
do { 
rang=rang+l1; 
d9=rang; 
computematrix(&xx1,&yy1,&221,&th4,&th5,&th6, &th7,&th8,&d9); 
1=(a0_9[1][4)/6): 
k=(-1)*a0_ 9/2][4]/6; 
celev=(-1)* (a0_9[3][4}); 
melev=array|k|(1]; 
} while(celev>melev); 


else 


{ 
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do { 
rang=rang-1; 
d9=rang; 
computematrix(&xx1,&yy1,&zz1,&th4,&th5,&th6, &th7,&th8 ,&d9); 
1=(a0_9[1)[4]/6); 
k=(-1)*a0_9[2][4]/6; 
celev=(-1)* (a0 _9/3][4]); 
melev=array|k]/]]; 
} while(celev<=melev); 
do { 
rang=rang+1; 
d9=rang; 
computematrix(&xx1,&yy1,&221,&th4,&th5,&th6,&th7,&th8,&d9); 
1=(a0_9[1}[4]/6) 
k=(-1)*a0_92|[4]/6; 
celev=(-1)* (a0_9(3][4}); 
melev=array|k| (I); 
} while(celev>melev); 


d9=d9-.5; /* back up range by .5 inch */ 
u=(a0 9[1][4]-0.5*a0 9/2][4]+30); 

v=(0.5*a0 9[2][4)+a0 9[3][4]+542); 
BM_SetPosition(bmd,u2,v2); 

BM_PaintLine(bmd,u,v); 

BM _DisplayBitmap(fd, PAINTRASTER,bmd,0,0,0,0,1220,664,0); 


ae ce SENSES RS Tt SSSR ESAS LANES SER EA LEE E SES 


Compute the noisy range value for init=1. 
Rn kage te tS SERRE EES TEESE f 


sig02=0.02; 
sig12—0.0001: 
n=noise(); 
sigscan2=sig02 + sig12*d9*d9Q; 
sigscan=sqrt(sigscan2); 
oimt =—1) 
{n=noise(); 
d9=d9+n‘*sigscan ; 
rier|=d9; 


else 
{rhat|rr|=d9; 


tt=tt+1; 

j= Fri 

} while(j<12); 
i=i+1; 
} while(i<36); 
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| hediaiiaiiedadindcl "Dadian st iad tadiadaa EN 


Start regression analysis. Set value of sigma (s) to 


be used in the cell masks. 
FERRE ESS EE REET Se arena ce eer. eels aaa 


s— 12 

g|0|=0; 

f=1; 

for(ii=1;i1<433;ii++) /* compute initial criterion function value,g|0| */ 
{ g[0]=g[0]+(r[ii]-rhat/ii]) *(r[ii]-rhat[ii]); 


d2min=d?2; 
d3min=d3; 
gmin=g(0); 
safe=1; 
kKk=1: 


[SESRERSE SESS SEES ES DES SS ee ee ee 


Change x and y coordinate values and simulate terrain 
scanning to obtain rhat|rr] to be able to compute 


criterion function values for each grid position. 
FES EE ESLER SES EET Ee Se a eee” 0 dane eer 


do { 
d2=d2min; 
d3=d3min; 
g[0|=gmin; 
for(ii=1;11<9;i1++) 
{if(ii==1) 

{d2=d2-s/f; 

d3=d3-s/f; 

} 
else if (ii==2) 
{d3=d3-s/f; 


else if (ii==3) 
{d2=d2+s/f: 
d3=d3-s /f; 


else if (ii==4) 
{d2=d2-s/f; 
} 

else if (ii==5) 


{d2=d2+s/f. 


else if (ii==6) 
{d2=d2-s/f, 
d3=d3+s/f; 

} 
else if (ii==7) 
{d3=d3+s/f; 
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else if (1==8) 
{d2=d2+s/f; 
d3=d3+s/f; 
} 
rr=0; 
i=1; 
do { 
if(i==1) 
{elev=(-1)*20.; 


else if (i==13) 
{elev=(-1)*22.; 


else if (i==25) 
{elev=(-1)*30.; 


th7=elev-1; 
tt=1; 
j=1; 
do { 
rr=rr+1; 
th8=6-}; 
if(tt==1) 
{rang=0; 
do { 
rang=rang+1; 
d9=rang; 
computematrix(&d1,&d2,&d3,&t4,&t5,&t6,&th7,&th8 &d9); 
1=(a0_9[1][4]/6) 
k=(-1)*a0_9/2][4)/6; 
celev=(-1)* (a0_9(3][4]); 
melev=array|k]|]]; 
} while(celev>melev); 


} 
else 


do { 

rang=rang-1; 

d9=rang; 
computematrix(&d1,&d2,&d3,&t4,&t5,&t6,&th7,&th8,&d9); 
1=(a0_9)1)[4]/6); 

k=(-1)*a0 9{2][4]/6; 

celev=(-1)* (a0 9(3][4]); 

melev=array/|k](I]; 

}while(celev<=melev); 

do { 

rang=rang+1; 

d9=rang; 
computematrix(&d1,&d2,&d3,&t4,&t5,&t6,&th7,&th8,&d9); 
1=(a0_9{1][4]/6); 

k=(-1)*a0_9(2|(4]/6; 

celev=(-1)* (a0 _9/3]/4)); 
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melev=array|k](l|; 
} while(celev>melev); 


d9=d9-.5; 
rhat|rr]=d9; 
tt=tt+1; 
J=)+1; 
} while(j<12) ; 
i=1+1; 
} while(i<36); 


[PETE SR EA NEA OSE De ine age tye See eo ca ene 


Compute criterion function for the appropriate 


cell mask. 
aida i acid adadadadaadadadae ae aad gag) 


[ii] =0; 
for(ij=1sii<433ei}+-+) 
{g[31]=g[3i] + (r[Jj|-rhatl(3j])*(r[ij|-rhat1 (jj); 


dx(ii]=d2; 
dy|ii]=d3; 
if(ui==1) 
{d2=d2+s/f; 
d3=d3+s/f; 


else if (ii==2) 
{d3=d3+s/f; 


else if (ii==3) 
{d2=d2-s/f; 
d3=d3+s/f; 


else if (ii==4) 
{d2=d2+s/f; 


else if (1i==5) 
{d2=d2-s/f; 


else if (ii==6) 
{d2=d2+s/f; 
d3=d3-s/f; 


} 
else if (ii==7) 
{d3=d3-s/f,; 


else if (ii==8) 
{d2=d2-s/f; 
d3=d3-s/f; 
} 
} 
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i tt STEERER ESSE SESE ESS 


Determine the minimum value of criterion function 


and its associated x,y coodinates. 
il adiladatad ali al laladaliadatladalaiad | 


for(jj=1;3)<9;jj++) 
{ilemin < eli) 
{d2min=d2min, 
d3min=d3min; 
gmin=gmin; 


} 


else 
{d2min=dx\|jj]; 
d3min=dy|jj]; 
gmin=g|jj]; 


} 


enna * Oia ee TEESE EA EER SR EES ERASE SESE EE 


If minimum value of criterion function is not 
in center cell, keep f=1 and increment safe by 1. 
If minimum is in center cell or safe=4, start 


increasing f by 2 times until its is > s. 
een Dette EET EES STEM ES TET ES EERE EERE RES | 


if(gmin != g[O] && kk==1 && safe !=4) 
{f=1; 
ci 1: 
safe=safe+ 1; 


else if (gmin == g[0}) 
oie 
kk=kk+41; 
safe=4; 
} 
else if (safe == 4) 
MF 2; 
kk=kk+1; 


} while(f<s) ; 
d2=d2min; 
d3=d3min; 


ana Te TREN EERE SELES SENSES ERLE SELES SEE ES 


Start computation of x,y, and z values for each 


azimuth and elevation value. 
a i allt laaciadadiladalitadiaiedia iad cadalucediiedadalaiadl | 
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{elev=(-1)*20.; 


} 
else if (i==13) 
{elev=(-1)*22.; 


else if (i==25) 
{elev=(-1)*30.; 


th7=elev -1; 
J=1; 
do { 
th8=6-}; 
=rr+l1; 
d9=r[rr]; 
computematrix(&xx1,&yyl1,&221,&th4,&th5,&th6, &th7,&th8,&d9); 
xx=a0_9[1][4]/6; 
yy=(-1)*a0_9[2][4]/6; 
zz|xx]|yy]=(-1)*a0_9[3][4]; 


eee ee ee Tee gee eae ee eee nee 


Perform Kalman filtering for each terrain 


cell scanned. 
salaalatialadh tad ldacadidiaiadadadad ios ad as 


sigz2|/xx|[yy|=(sig02 + sig12*d9*d9)*sin(th7*3.14159/180) 
*sin(th7*3.14159/180)+.232; 
if(sigzold2|xx|[yy|==0) 
{sigzold2[xx]{yy]=144000.; 


zhat|xx]|yy]=zhat|xx]|yy]+ (sigzold2[xx|[yy]/(sigzold2[xx]|yy|+sigz2[xx|[yy])) 
* (22|[xx][yy]-zhat[xx|[yy]); 
sigzold2{xx]||yy]=sigz2[xx|[yy]*sigzold2[xx]|yy]/(sigz2[xx||yy]+sigzold2/xx|[yy]); 
jJ=)+1; 
} while(j<12); 
i=1+1; 
} while(i<36); 
} while(scan<8); 
printf(" program complete "); 
while (1) 
A 
} 


KKKKKKKEKKKKKKKAKKEKAKEKKKEKAKKAKEKKEKKEKEKEKEKKKAKKEKKKEKEKKE KS 


End of the main program 
Ree ee 


[HERE REDE ES EEE EERE ER ee ia eee 


Subroutine to perform the coordinate transformation 
using the D-H transformation. Only the values of the 


matrices needed are computed. 
SEE RES EERE EES SESE Ea EEE EE tO eee ea 
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computematrix(x1,y1,z1,tth4,tth5,tth6,tth7,tth8,dd9) 
float *x1,*y1,*z1,*tth4, *tth5,*tth6, *tth7, *tth8,*dd9Q; 


float c4,c5,c6,c7,c8,thact4,thact5,thact6,th7,th8,d9; 

float s4,s5,s6,s7,s8,xxl,yyl,zz1; 

int a7; 

aol =(*x1); 

yyl=(*y1); 

as1—(*2z1); 

thact4=(*tth4); 

thact5=(*tth5); 

thact6=(*tth6); 

th7=(*tth7); 

th8=(*tth8); 

d9=(*dd9); 

c4=cos(thact4*3.1416/180 +3.1416); 

c5=cos(thact5*3.1416/180 -1.5708); 

c6=cos(thact6*3.1416/180 +3.1416); 

c7=cos(th7*3.1416/180 -1.5708); 

c8=cos(th8*3.1416/180 -1.5708); 

s4=sin(thact4*3.1416/180 + 3.1416); 

s5=sin(thact5*3.1416/180 -1.5708); 

s6=sin(thact6*3.1416/180 + 3.1416); 

s7=sin(th7*3.1416/180 - 1.5708); 

s8=sin(th8*3.1416/180 - 1.5708); 

a7=(-1)*5; 

aQ 9{1][/4]=xx1+(c4*c5*c6+s4*s6) *(d9*c7*s8+a7*c7)+(c4*s5)*(d9*s7*s8+a7*s7) 
+(c4*c5*s6-s4*c6)*(c8*d9); 

a0 9[2]|4]=yy1+(s4*c5*c6-c4*s6)* (d9*c7*s8+a7*c7)+(s4*35)*(d9*s8*s7+a7*s7) 
+(s4*c5*s6+c6*c4)* (c8*d9); 

a0 _9/3][4|=2z1+(s5*c6)*(d9*c7*s8+a7 *c7)-(d9*s8*s7+a7*s7) *(c5) 
+(s5*s6)*(c8*d9); 


i Amie or eee eS SESSA SS EE SESS SER SE SESE ES 


Subroutines to do x and y transformations used in the 


terrain drawing portion of the program. 
Pn a SEES SE ESET EES ESSE SELES | 


xtrfunc(xint,x,y) 

long int *x,*y ; 

inte xint; 

{ 
int Xx; 
xx=(*xint)+(*x)+(*y)*.5; 
return XX; 


} 


ytrfunc(yint,y,z) 
long int *y,*z ; 
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int *yint; 

{ 
int yy; 
yy=(*yint)-(*y)*.5-(*z); 
return yy; 


} 


[ARERR EE A ne ec ce a ae aa a 


Random number generator used in creating terrain. 
tHE AAR RARE EAE EASE AAR EAE EE SESE AES EASES EERE EASE AES / 


ran(r) 
int r; 
{ 
int ran; 
seed=(seed2+3.1) * (seed2+3.1); 
seed 1=seed; 
seed2=seed-seed 1; 
ran=(r )*(seed2) + 1; 
return (ran); 


} 


| haat diadatialadindiad da ita a so ao aa 


Subroutine used to create a random gaussian number 


with mean=0. 
TE rae ae cr eae aaa 


float 
noise() 
{ 
float b,n,seed,x,xx; 
int 1,seed1; 
b=0.5; 
xx=0; 
for(i=1;i1<13;++i) 
{seed=(seed3+3.141634) *(seed3+3.141634); 
seed 1=seed; 
seed3=seed - seed1; 
x=2*seed3 - 1; 
XX=XX + X; 
} 
n=b*xx; 
return(n); 


} 
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